#ifndef CONFIG_HPP_
#define CONFIG_HPP_
#include <glog/logging.h>
#include <termios.h>
constexpr float kPI = 3.1415926f;
template <typename T> T DegToRad(const T &deg) { return T(kPI * deg / 180.); }
template <typename T> T RadToDeg(const T &rad) { return T(180. * rad / kPI); }

template <typename T> T RadRange(const T &rad) {
    T result = rad;
    while (result >= T(2 * kPI))
        result -= 2 * kPI;
    while (result < T(0))
        result += 2 * kPI;
    return result;
}

template <typename T> T RadNorm(const T &rad) {
    T result = rad;
    while (result >= T(kPI))
        result -= 2 * kPI;
    while (result < T(-kPI))
        result += 2 * kPI;
    return result;
}

constexpr termios kUltrasonicSerialOptions = {
    .c_iflag = uint16_t(IGNPAR) |
               uint16_t(~IGNBRK) & uint16_t(~BRKINT) & uint16_t(~PARMRK) &
                   uint16_t(~INPCK) & uint16_t(~ISTRIP) & uint16_t(~INLCR) &
                   uint16_t(~IGNCR) & uint16_t(~ICRNL) & uint16_t(~IUCLC) &
                   uint16_t(~IXON) & uint16_t(~IXANY) & uint16_t(~IXOFF) &
                   uint16_t(~IMAXBEL) & uint16_t(~IUTF8),
    .c_oflag = uint16_t(ONLCR) | uint16_t(NLDLY & NL0) | uint16_t(CRDLY & CR0) |
               uint16_t(TABDLY & TAB0) | uint16_t(BSDLY & BS0) |
               uint16_t(VTDLY & VT0) |
               uint16_t(FFDLY & FF0) & uint16_t(~OPOST) & uint16_t(~OLCUC) &
                   uint16_t(~OCRNL) & uint16_t(~ONOCR) & uint16_t(~ONLRET) &
                   uint16_t(~OFILL) & (uint16_t)~OFDEL,
    .c_cflag = uint16_t(B115200) | uint16_t(CS8) | uint16_t(CREAD) |
               uint16_t(HUPCL) |
               uint16_t(CLOCAL) & uint16_t(~CSTOPB) & uint16_t(~PARENB) &
                   uint16_t(~PARODD) |
               CRTSCTS,
    .c_lflag = uint16_t(ECHOCTL) | uint16_t(PENDIN) |
               uint16_t(ECHOKE) & uint16_t(~ECHO) & uint16_t(~ICANON) &
                   uint16_t(~ISIG) & uint16_t(~XCASE) & uint16_t(~ECHOE) &
                   uint16_t(~ECHOK) & uint16_t(~ECHONL) & uint16_t(~ECHOPRT) &
                   uint16_t(~FLUSHO) & uint16_t(~NOFLSH) & uint16_t(~TOSTOP) &
                   uint16_t(~IEXTEN),
    .c_line = uint8_t(0),
    .c_cc = {1, 1, 8, 1, 1, 0, 0, 0, 1, 1, 1, 0, 1, 0, 1, 1, 0},
};

constexpr int64_t kPerReadTimeoutns = 100000; // ns 0.1ms
constexpr int kMaxSerialBuf = 115200;
constexpr uint8_t kUltrasonicFrameHeader = 0xFF;
constexpr uint8_t kUltrasonicStartCode = 0xAA;
const std::vector<std::string> kUltrasonicPorts = {
    "/dev/Ultrasonic_fl", "/dev/Ultrasonic_fr", "/dev/Ultrasonic_bl",
    "/dev/Ultrasonic_br", "/dev/Ultrasonic_l",  "/dev/Ultrasonic_r"};
constexpr int kUltrasonicFrequence = 30;

const std::vector<std::string> kLinelaserPorts = {
    "/dev/line_laser_b", "/dev/line_laser_f", "/dev/line_laser_l",
    "/dev/line_laser_r"};

const std::string kImuPort = "/dev/imu";
const std::string kEncodePort = "/dev/motor";

constexpr int kLinelaserFrequence = 30;
constexpr int kBandrateCode115200 = B115200;
constexpr int kBandrateCode230400 = B230400;
constexpr int kBandrateCode921600 = B921600;
constexpr int kBandrateCode1500000 = B1500000;

constexpr size_t kLinelaserCommandLength = 0;
constexpr size_t kLinelaserDataLength = 322;
constexpr uint8_t kLinelaserHearder = 0xA5;

constexpr uint8_t kLinelaserAddress1 = 0x01;
constexpr uint8_t kLinelaserAddress2 = 0x02;
constexpr uint8_t kLinelaserAddress3 = 0x04;

constexpr uint8_t kLinelaserGetAddress = 0x60;
constexpr uint8_t kLinelaserGetParameter = 0x61;
constexpr uint8_t kLinelaserGetVersion = 0x62;
constexpr uint8_t kLinelaserScanCode = 0x63;
constexpr uint8_t kLinelaserStopScan = 0x64;
constexpr uint8_t kLinelaserRestartScan = 0x67;
constexpr uint8_t kLinelaserSetMode = 0x69;
constexpr uint8_t kLinelaserSetBias = 0xD9;

const std::vector<uint8_t> kLinelaserGetAddressCommand = {
    0xA5, 0xA5, 0xA5, 0xA5, 0x00, 0x60, 0x00, 0x00, 0x60};

const std::vector<uint8_t> kLinelaserGetParameterCommand = {
    0xA5, 0xA5, 0xA5, 0xA5, 0x00, 0x61, 0x00, 0x00, 0x61};

const std::vector<uint8_t> kLinelaserGetVersionCommand = {
    0xA5, 0xA5, 0xA5, 0xA5, 0x00, 0x62, 0x00, 0x00, 0x62};

const std::vector<uint8_t> kLinelaserStartCommand = {
    0xA5, 0xA5, 0xA5, 0xA5, 0x00, 0x63, 0x00, 0x00, 0x63};

const std::vector<uint8_t> kLinelaserStopCommand = {
    0xA5, 0xA5, 0xA5, 0xA5, 0x00, 0x64, 0x00, 0x00, 0x64};

const std::vector<uint8_t> kLinelaserRestartCommand = {
    0xA5, 0xA5, 0xA5, 0xA5, 0x01, 0x67, 0x00, 0x00, 0x68};

const std::vector<uint8_t> kLinelaserSet921600 = {0xA5, 0xA5, 0xA5, 0xA5, 0x00,
                                                  0x68, 0x01, 0x00, 0x02, 0x6A};

const std::vector<uint8_t> kLinelaserSet230400 = {0xA5, 0xA5, 0xA5, 0xA5, 0x00,
                                                  0x68, 0x01, 0x00, 0x01, 0x6A};

constexpr int kImuFrequence = 100;
constexpr uint8_t kImuHeader = 0X3A;
constexpr uint8_t kImuEnd1 = 0x0D;
constexpr uint8_t kImuEnd2 = 0x0A;

constexpr int kCameraFrequence = 10;

const std::vector<std::string> kCameraPorts = {"/dev/camera_up",
                                               "/dev/camera_down"};
const std::string kCameraConfigFile =
    "/home/pi/ikitrobot_one/install/share/check/config/config.json";

const std::string kLidarPort = "/dev/ttyS4";
constexpr int kLidarFrequence = 200;
const std::vector<uint8_t> kLidarStartCommand = {0x4C, 0x53, 0x54,
                                                 0x41, 0x52, 0x48};
const std::vector<uint8_t> kLidarStopCommand = {0x4C, 0x53, 0x54,
                                                0x4F, 0x50, 0x48};
// const char kLidarStopCommand1[] = "LSTOPH";
const std::vector<uint8_t> kLidarRestartCommand = {0x4C, 0x52, 0x45,
                                                   0x53, 0x54, 0x48};
const std::vector<uint8_t> kLidarGetUUIDCommand = {0x4C, 0x55, 0x55,
                                                   0x49, 0x44, 0x48};
const std::vector<uint8_t> kLidarGetTypeCommand = {0x4C, 0x54, 0x59,
                                                   0x50, 0x45, 0x48};
constexpr uint8_t kLidarHearder1 = 0xCF;
constexpr uint8_t kLidarHearder2 = 0xFA;

constexpr float kCameraUpOffsetX = 0.2653;
constexpr float kCameraUpOffsetY = 0.;
constexpr float kCameraUpOffsetZ = 0.4586;
constexpr float kCameraUpOffsetRoll = 0.;
constexpr float kCameraUpOffsetPitch = 0.;
constexpr float kCameraUpOffsetYaw = 0.;
constexpr float kCameraDownOffsetX = 0.3193;
constexpr float kCameraDownOffsetY = 0.;
constexpr float kCameraDownOffsetZ = 0.1156;
constexpr float kCameraDownOffsetRoll = 0.;
constexpr float kCameraDownOffsetPitch = 0.;
constexpr float kCameraDownOffsetYaw = 0.;
constexpr size_t kCameraSampleSize = 8;

#endif